/**
 * Copyright 2021 xpstem.com
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

package com.xpstem.lelink;

import com.xpstem.lelink.command.EV3Command;
import com.xpstem.lelink.command.EV3DirectCommand;
import com.xpstem.lelink.command.EV3ReplyCallback;
import com.xpstem.lelink.command.EV3SystemCmdFactory;
import com.xpstem.lelink.connection.Connection;
import com.xpstem.lelink.device.*;
import com.xpstem.lelink.enums.*;
import com.xpstem.lelink.operation.EV3Operation;
import com.xpstem.lelink.operation.EV3InputOpFactory;
import com.xpstem.lelink.operation.EV3OutputOpFactory;
import com.xpstem.lelink.operation.ReturnParam;
import com.xpstem.lelink.type.Data16;
import com.xpstem.lelink.type.Data32;
import com.xpstem.lelink.type.Data8;

import java.io.IOException;
import java.lang.reflect.Constructor;
import java.lang.reflect.InvocationTargetException;
import java.util.*;
import java.util.concurrent.Executors;
import java.util.concurrent.ScheduledExecutorService;

/**
 * EV3 programmable brick.
 *
 * @author billy zhang
 */
public class EV3BrickClient implements EventBus {

    private ScheduledExecutorService schService;

    public interface DeviceListener {

        default void onValueChanged(InputPort port, EV3Sensor sensor) {}

        default void onLink(InputPort port, EV3InputDevice inputDevice) {}

        default void onLink(OutputPort port, EV3OutputDevice outputDevice) {}

        default void onLost(InputPort port, EV3InputDevice inputDevice) {}

        default void onLost(OutputPort port, EV3OutputDevice outputDevice) {}
    }

    private Connection conn;
    private Optional<DeviceListener> deviceListener = Optional.empty();

    private static Map<Short, Class<? extends Device>> supportDeviceMap =
            new HashMap<>();

    static {
        supportDeviceMap.put(EV3ColorSensor.TYPE, EV3ColorSensor.class);
        supportDeviceMap.put(EV3GyroSensor.TYPE, EV3GyroSensor.class);
        supportDeviceMap.put(EV3IRSensor.TYPE, EV3IRSensor.class);
        supportDeviceMap.put(EV3UltrasonicSensor.TYPE, EV3UltrasonicSensor.class);
        supportDeviceMap.put(EV3TouchSensor.TYPE, EV3TouchSensor.class);
        supportDeviceMap.put(EV3LargeMotor.TYPE, EV3LargeMotor.class);
        supportDeviceMap.put(EV3MediumMotor.TYPE, EV3MediumMotor.class);
    }

    /**
     * Port 1 - 4
     */
    private EnumMap<InputPort, EV3InputDevice> inputDeviceMap;
    /**
     * Port A - D
     */
    private EnumMap<OutputPort, EV3OutputDevice> outputDeviceMap;

    private Layer layer;

    public EV3BrickClient(Layer layer) {
        this.layer = layer;
        schService = Executors.newSingleThreadScheduledExecutor();
        inputDeviceMap = new EnumMap<>(InputPort.class);
        outputDeviceMap = new EnumMap<>(OutputPort.class);
    }

    @Override
    public Layer getLayer() {
        return layer;
    }

    public void setDeviceListener(DeviceListener listener) {
        this.deviceListener = Optional.ofNullable(listener);
    }

    @Override
    public void postCommand(EV3Command command) {
        executeCommand(command);
    }

    @Override
    public void postCommand(EV3Command command, EV3ReplyCallback callback) {
        executeCommand(command, callback);
    }

    @Override
    public void onSensorChanged(InputPort portNo, EV3Sensor sensor) {
        deviceListener.ifPresent(l->l.onValueChanged(portNo, sensor));
    }

    @Override
    public void onDeviceLost(InputPort portNo, EV3InputDevice device) {
        inputDeviceMap.remove(portNo);
        if (device instanceof EV3MotorSensor) {
            OutputPort outPort = ((EV3MotorSensor)device).getMotor().getOutPort();

            deviceListener.ifPresent(l->l.onLost(outPort, outputDeviceMap.get(outPort)));

            outputDeviceMap.remove(outPort);
        } else {
            deviceListener.ifPresent(l->l.onLost(portNo, device));
        }
    }

    public void connect(Connection conn) {
        this.conn = conn;
        listDevices();
    }

    public void refresh() {
        listDevices();
    }

    public Map<InputPort, EV3InputDevice> getInputDeviceList() {
        return inputDeviceMap;
    }

    public Map<OutputPort, EV3OutputDevice> getOutputDeviceList() {
        return outputDeviceMap;
    }

    public EV3OutputDevice getOutputDevice(OutputPort port) {
        if (outputDeviceMap.containsKey(port)) {
            return outputDeviceMap.get(port);
        }

        return null;
    }

    public EV3InputDevice getInputDevice(InputPort port) {
        if (inputDeviceMap.containsKey(port)) {
            return inputDeviceMap.get(port);
        }

        return null;
    }

    public void executeCommand(EV3Command cmd) {
        try {
            conn.send(cmd);
        } catch (IOException ex) {
            ex.printStackTrace();
        }
    }

    public void executeCommand(EV3Command cmd, EV3ReplyCallback callback) {
        try {
            conn.send(cmd, callback);
        } catch (IOException ex) {
            ex.printStackTrace();
        }
    }

    private void listDevices() {

        try {
            int len = 8;
            ReturnParam<Data8> array = new ReturnParam<>(0, Data8.from(0), new Data8[len]);
            ReturnParam<Data8> changed = new ReturnParam<>(len, Data8.from(0));
            EV3Operation listOp = EV3InputOpFactory.opInputDeviceList(Data8.from(len), array, changed);
            EV3DirectCommand cmd = new EV3DirectCommand(listOp);
            executeCommand(cmd);

            for (int i = 0; i < array.getValues().length; i++) {

                short type = (short)array.getValues()[i].getValue();
                if (type == EV3InputOpFactory.PORT_EMPTY_OR_UNAVAILABLE) {
                    continue;
                }

                if (supportDeviceMap.containsKey(type)) {
                    addInputDevice(i, type);
                }

            }
        } catch (IOException ex) {
            ex.printStackTrace();
        }

        List<OutputPort> outPortList = Arrays.asList(OutputPort.A, OutputPort.B, OutputPort.C, OutputPort.D);

        try {
            for (OutputPort outPort : outPortList) {
                addOutputDevice(outPort);
            }
        } catch (IOException ex) {
            ex.printStackTrace();
        }
    }

    private void addOutputDevice(OutputPort outPort) throws IOException {
        ReturnParam<Data8> typeParam = new ReturnParam<>(0, Data8.from(0));
        ReturnParam<Data8> modeParam = new ReturnParam<>(1, Data8.from(0));
        EV3Operation op = EV3InputOpFactory.OpInputDevice.getTypeMode(layer, outPort.getInputPort(), typeParam, modeParam);
        EV3DirectCommand cmd = new EV3DirectCommand(op);
        executeCommand(cmd);

        int type = typeParam.getValue().getValue();
        if ( type == EV3InputOpFactory.PORT_EMPTY_OR_UNAVAILABLE) {
            return;
        }

        Class<? extends Device> clazz = supportDeviceMap.get((short)type);
        if (EV3OutputDevice.class.isAssignableFrom(clazz)) {
            try {
                Constructor[] arr = clazz.getConstructors();

                short mode = (short) modeParam.getValue().getValue();
                //Constructor ci = clazz.getConstructor(new Class[] {EV3BrickClient.class, InputPort.class, Short.class});
                EV3OutputDevice device = (EV3OutputDevice) arr[0].newInstance(this, outPort, mode);
                outputDeviceMap.put(outPort, device);
                if (device instanceof EV3ServoMotor) {
                    EV3MotorSensor sensor =  ((EV3ServoMotor) device).getSensor();
                    inputDeviceMap.put(outPort.getInputPort(), sensor);
                    deviceListener.ifPresent(l->l.onLink(outPort.getInputPort(), sensor));
                }
                deviceListener.ifPresent(l->l.onLink(outPort, device));

            } catch (InvocationTargetException e) {
                e.printStackTrace();
            } catch (InstantiationException e) {
                e.printStackTrace();
            } catch (IllegalAccessException e) {
                e.printStackTrace();
            }
        }
    }

    private void addInputDevice(int portNo, short type) throws IOException {
        InputPort inPort = InputPort.ofValue(portNo);
        ReturnParam<Data8> typeParam = new ReturnParam<>(0, Data8.from(0));
        ReturnParam<Data8> modeParam = new ReturnParam<>(1, Data8.from(0));
        EV3Operation op = EV3InputOpFactory.OpInputDevice.getTypeMode(layer, inPort, typeParam, modeParam);
        EV3DirectCommand cmd = new EV3DirectCommand(op);
        executeCommand(cmd);

        if (type != typeParam.getValue().getValue()) {
            throw new IOException("device type wrong.");
        }
        Class<? extends Device> clazz = supportDeviceMap.get(type);
        if (EV3InputDevice.class.isAssignableFrom(clazz)) {
            try {
                Constructor[] arr = clazz.getConstructors();

                short mode = (short)modeParam.getValue().getValue();
                //Constructor ci = clazz.getConstructor(new Class[] {EV3BrickClient.class, InputPort.class, Short.class});
                EV3InputDevice device = (EV3InputDevice)arr[0].newInstance(this, inPort, mode);
                inputDeviceMap.put(inPort, device);
                deviceListener.ifPresent(l->l.onLink(inPort, device));

            } catch (InvocationTargetException e) {
                e.printStackTrace();
            } catch (InstantiationException e) {
                e.printStackTrace();
            } catch (IllegalAccessException e) {
                e.printStackTrace();
            }
        }
    }

    public <T> short writeMailbox(String mailboxName, T value) {
        // TODO:
        EV3SystemCmdFactory.WriteMailboxCommand cmd = new EV3SystemCmdFactory.WriteMailboxCommand<>(mailboxName, value);
        executeCommand(cmd);
        return 0;
    }

    public short setBluetoothPin(String macAddr, String pinCode) {
        // TODO:
        EV3SystemCmdFactory.SetBluetoothPinCommand cmd = new EV3SystemCmdFactory.SetBluetoothPinCommand(
                macAddr, pinCode);
        executeCommand(cmd);
        return 0;
    }

}
